package com.grt192.controller.breakaway.test;

import com.grt192.core.EventController;
import com.grt192.event.GlobalEvent;
import com.grt192.event.GlobalListener;
import com.grt192.event.SensorEvent;
import com.grt192.event.component.ButtonListener;
import com.grt192.mechanism.GRTDriverStation;
import com.grt192.mechanism.breakaway.GRTBreakawayRobotBase;
import com.grt192.mechanism.breakaway.KickerOmega;
import com.grt192.mechanism.breakaway.Recovery;
import com.grt192.mechanism.breakaway.Roller;
import com.grt192.sensor.GRTJoystick;
import edu.wpi.first.wpilibj.templates.MainRobot;


/**
 * 
 * @author anand
 */
public class MechanismTestController extends EventController implements
		ButtonListener, GlobalListener {

	public static final int IDLE = 0;
	public static final int LEFT_DT_FORWARD = 1;
	public static final int RIGHT_DT_FORWARD = 3;
	public static final int LEFT_DT_REVERSE = 2;
	public static final int RIGHT_DT_REVERSE = 4;
	public static final int KICKER = 5;
	public static final int LEFT_ROLLER_GRAB = 6;
	public static final int LEFT_ROLLER_PUSH = 7;
	public static final int RIGHT_ROLLER_GRAB = 8;
	public static final int RIGHT_ROLLER_PUSH = 9;
	public static final int RECOVERY_EXTEND = 10;
	public static final int RECOVERY_RETRACT = 11;
	public static final int FINISH_TEST = 12;

	private Roller rollers;
	private KickerOmega kicker;
	private GRTBreakawayRobotBase driveTrains;
	private Recovery recovery;
	private GRTDriverStation ds;

	private int state;
	private boolean[] testScore;

	public MechanismTestController(Roller rollers, KickerOmega kicker,
			GRTBreakawayRobotBase driveTrains, Recovery recovery,
			GRTDriverStation ds) {
		super();
		this.rollers = rollers;
		this.kicker = kicker;
		this.driveTrains = driveTrains;
		this.recovery = recovery;
		this.ds = ds;
		((GRTJoystick) ds.getSensor("leftJoystick")).addButtonListener(this);
		addMechanism("DriveTrain", driveTrains);
		addMechanism("Rollers", rollers);
		addMechanism("Recovery", recovery);
		addMechanism("Kicker", kicker);
		addMechanism("DriverStation", ds);
                MainRobot.getInstance().addGlobalListener(this);
		state = IDLE;
		testScore = new boolean[11];
	}

	protected void printToAll(String text) {
		System.out.println(text);
		ds.printToLCD(text);
	}

	public void buttonUp(SensorEvent e, String key) {
		if (key.equals("Button8") || key.equals("Button9")
				|| key.equals("Button7")) {
			if (key.equals("Button8")) {
				// Pass Button
				if (state != IDLE) {
					testScore[state - 1] = true;
					printToAll("PASS");
				}
				state++;

			} else if (key.equals("Button9")) {
				// Fail Button
				if (state != IDLE) {
					testScore[state - 1] = false;
					printToAll("Failed Test: " + state);
					state++;
				}
			} else if (key.equals("Button7")) {
				// Stop test
				if (state != IDLE) {
					state = FINISH_TEST;
					printToAll("Halting Test");
				}
			}
			switch (state) {

			case LEFT_DT_FORWARD:
				// Test left DT
				driveTrains.stop();
				driveTrains.tankDrive(-1.0, 0, 1000);
				printToAll("Left DT forward");
				break;
			case LEFT_DT_REVERSE:
				driveTrains.stop();
				driveTrains.tankDrive(1.0, 0.0, 1000);
				printToAll("Left DT reverse");
				break;
			// Test right DT
			case RIGHT_DT_FORWARD:
				driveTrains.stop();
				driveTrains.tankDrive(0, -1.0, 1000);
				printToAll("Right DT forward");
				break;

			case RIGHT_DT_REVERSE:
				driveTrains.stop();
				driveTrains.tankDrive(0.0, 1.0, 1000);
				printToAll("Right DT reverse");
				break;

			case KICKER:
				driveTrains.stop();
				// Test Kicker
				kicker.kick();
				printToAll("Kicker Fired");
				break;

			case LEFT_ROLLER_GRAB:
				kicker.stopKick();
				// Test left Roller
				rollers.leftGrab();
				printToAll("Left Roller Grab");
				break;

			case LEFT_ROLLER_PUSH:
				rollers.stopLeftRoller();
				rollers.leftPush();
				printToAll("Left Roller Push");
				break;

			case RIGHT_ROLLER_GRAB:
				rollers.stopLeftRoller();
				// Test right Roller
				rollers.rightGrab();
				printToAll("Right Roller Grab");
				break;

			case RIGHT_ROLLER_PUSH:
				rollers.stopRightRoller();
				rollers.rightPush();
				printToAll("Right Roller Push");
				break;

			case RECOVERY_EXTEND:
				rollers.stopRightRoller();
				// Test Recovery
				recovery.flip();
				printToAll("Recovery Extend");
				break;
			case RECOVERY_RETRACT:
				recovery.stopArm();

				recovery.retract();
				printToAll("Recovery Retract");
				break;

			case FINISH_TEST:
				recovery.stopArm();
				rollers.stop();
				driveTrains.stop();
				kicker.stopKick();
				state = IDLE;
				for (int i = 0; i < testScore.length; i++) {
					String scoreLine = "";
					switch (i + 1) {
					case LEFT_ROLLER_PUSH:
						scoreLine = ("Left Roller Push: ");
						break;
					case LEFT_ROLLER_GRAB:
						scoreLine = ("Left Roller Grab: ");
						break;
					case RIGHT_ROLLER_PUSH:
						scoreLine = ("Right Roller Push: ");
						break;
					case RIGHT_ROLLER_GRAB:
						scoreLine = ("Right Roller Grab: ");
						break;

					case LEFT_DT_REVERSE:
						scoreLine = ("Left DT Reverse: ");
						break;
					case RIGHT_DT_REVERSE:
						scoreLine = ("Right DT Reverse: ");
						break;
					case LEFT_DT_FORWARD:
						scoreLine = ("Left DT Forward: ");
						break;
					case RIGHT_DT_FORWARD:
						scoreLine = ("Right DT Forward: ");
						break;
					case KICKER:
						scoreLine = ("Kicker: ");
						break;

					case RECOVERY_RETRACT:
						scoreLine = ("Recovery Retract: ");
						break;

					case RECOVERY_EXTEND:
						scoreLine = ("Recovery Extend: ");
						break;
					}
					printToAll(scoreLine +(testScore[i] ? "PASS" : "FAIL"));
				}
				break;

			case IDLE:
				break;
			}

		}

	}

	public void buttonDown(SensorEvent e, String key) {
	}

    public void globalChanged(GlobalEvent e) {
        if(e.getKey().equals("Angle")){
            printToAll("H "+MainRobot.getInstance().getGlobal(e.getKey()));
        }
    }

}
